#include <dispatch_system/time_based_astar.h>
#include <ros/ros.h>
#include <schedule_config/config.hpp>

#define START_REC_TIME 
#define END_REC_TIME(i) 

auto start_time = std::chrono::system_clock::now();

//膨胀区cell的cost
#define INFLATE_COST 3

namespace moying
{
namespace navigation
{
    int max_t = 0;
    TimeBasedAStar::TimeBasedAStar()
    {        
    }

    TimeBasedAStar::~TimeBasedAStar()
    {
        for(int i = 0; i < global_node_size_; i++)
        {
            if(nodes_2d_[i])
                delete nodes_2d_[i];
        }

        for(int i = 0; i < local_node_size_; i++)
        {
            if(nodes_3d_[i])
                delete nodes_3d_[i];
        }
    }

    bool TimeBasedAStar::init(const MoyingMapPtr &moying_map){

        cfg_info_ = sch::ScheduleConfig::instance()->getConfigInfo();
        drive_on_side_ =  cfg_info_.traffic_manager_cfg.drive_on_side;
        if(moying_map == nullptr || moying_map->getCostMap() == nullptr)
            return false;

        moying_map_ = moying_map;
        costmap_ = moying_map->getCostMap();

        MapDimension d = moying_map_->getDimensions();
        int ssm_ = d.sm_state;
        global_node_size_ = ssm_ > 0 ? d.x*d.y*d.theta*d.sm_state : d.x*d.y*d.theta;
        printf("global size:%d\n",sizeof(NodePtr)*global_node_size_);
        // nodes_2d_.resize(node_size);
        closed_2d_.resize(global_node_size_, false);
        if(nodes_2d_ != nullptr)
            delete nodes_2d_;
        nodes_2d_ = new NodePtr[global_node_size_];
        memset(nodes_2d_,0,sizeof(NodePtr)*global_node_size_);
        nodes_ptr_  = &nodes_2d_;
        closed_ptr_ = &closed_2d_;
        if(local_cost_map_ == nullptr)
            local_cost_map_ = std::make_shared<LocalCostMap>();
        local_cost_map_->init(moying_map);
        
        HIGHLIGHT("Node size "<<global_node_size_);
        return true;
    }


    unsigned int TimeBasedAStar::toIndex(int x,int y,int theta,int time,int move_state){

        MapDimension &d = dimension_;
        if(d.time == 0)
            time = 0;
        return x + d.x * y + d.x * d.y * theta + d.x * d.y * d.theta * move_state + d.x * d.y * d.theta * d.sm_state * time;

    }

    void TimeBasedAStar::resetNodes()
    {
        closed_ptr_->reset();
        open_.clear();
        path_start_nodes_.clear();
        path_end_nodes_.clear();
    }

    bool TimeBasedAStar::searchPath(const VehiclePtr &vehicle, const VehicleState &start, const VehicleState &goal, PlanningResult &result)
    {

        if(vehicle == nullptr || moying_map_ == nullptr)
            return false;

        move_patterns_in_map_ = moying_map_->get_custom_move_patterns(vehicle);
        if(move_patterns_in_map_ == nullptr){
            WARNING("can't get move patterns in map , robot_id:"<<vehicle->getID());
            return false;
        }
        vehicle_ = vehicle;

        // float min_dis = 10000;
        // moying_map_->getRobotMinDisToObstacle(vehicle->getID(),min_dis);
        // if(min_dis < 0.10){
        //     LOG_WARN("too close to the obstacle,robot_id:"<<vehicle->getID());
        //     vehicle_->set_last_path_plan_result(5);
        //     return false;
        // }

        // Check if we are on the map
        double wx = start.x;
        double wy = start.y;
        int start_x, start_y, goal_x, goal_y;

        if (!costmap_->worldToMap(wx, wy, start_x, start_y)) 
        {
            WARNING("The robot's start position is off the global costmap. Planning will always fail, are you sure the robot has been properly localized?");
            return false;
        }

        wx = goal.x;
        wy = goal.y;

        if (!costmap_->worldToMap(wx, wy, goal_x, goal_y)) 
        {
            WARNING("The goal sent to the global planner is off the global costmap. Planning will always fail to this goal.");
            return false;
        }

        std::vector<Coordinate> coor_vec;
        if( moying_map_->checkCollideLocalObstacle(vehicle_->getID(),goal.x,goal.y,goal.th,&coor_vec) ){
            WARNING("The goal can't reach cause it's been occupied by local obstacle. goal_x:"<<wx<<" goal_y:"<<wy<<" goal_theta:"<<goal.th);
            std::string coor_str;
            for(auto coor : coor_vec){
                coor_str += "x:" + std::to_string(coor.x) + " y:" + std::to_string(coor.y)+" , ";
            }
            vehicle_->set_last_path_plan_result(2);
            WARNING("collide polygon vertexex:"<<coor_str);
            return false;
        }


        if(costmap_->getCost(goal_x, goal_y) != MAPCOSTS::FREE){
            auto ccs = moying_map_->corrected_cells(vehicle_);
            if(ccs != nullptr){
                auto cci = (*ccs)[goal_x][goal_y];
                int theta = vehicle_->getCloseTheta(goal.th);
                if( !( cci.cell_x == goal_x 
                    && cci.cell_y == goal_y 
                    && (1<< theta & cci.theta) ) 
                ){ 
                    vehicle_->set_last_path_plan_result(2);
                    //目标点不是空闲区域,也不是可通行的膨胀区域
                    WARNING("The goal point is in obstacle cci.cell_x:"<<cci.cell_x<<" cci.cell_y:"<<cci.cell_y<<
                            " theta:"<<theta<<" robot_id:"<<vehicle->getID());
                    return false; 
                }
            }
        }

        dimension_ = moying_map_->getDimensions();
        dimension_.time = 0;
        nodes_ptr_  = &nodes_2d_;
        closed_ptr_ = &closed_2d_;
        resetNodes();

        HIGHLIGHT("Planning from "<<start_x<<" "<<start_y<<" to "<<goal_x<<" "<<goal_y<<" "<<time(0)<<" robot_id:"<<vehicle_->getID());
        
        
        
        //起始节点
        int theta_idx = vehicle_->getCloseTheta(start.th);
        auto all_owr = moying_map_->all_one_way_road();

        auto ccs = moying_map_->corrected_cells(vehicle_->getLength(),vehicle_->getWidth());
        if(ccs != nullptr){
            CorrectCellInfo cci = (*ccs)[start_x][start_y];
            if( !( cci.theta & (1<<theta_idx) ) ){ //如果起始点朝向无效,且在单向道内,设置为最接近的单向道朝向

                // //如果起点在单向道内，把起始朝向设为单向道的朝向
                for(auto owr : *all_owr){
                    if(owr->isCellInRoad(MapCell(start_x,start_y))){
                        theta_idx =owr->getCloseTheta(theta_idx);
                        HIGHLIGHT("set theta to one way road theta, th:"<< theta_idx<<" robot_id:"<<vehicle_->getID());
                        break;
                    }
                }

            }
        }
        
        int start_t = 1;
        unsigned int idx = toIndex(start_x, start_y, theta_idx, start_t,0);
        if((*nodes_ptr_)[idx] == nullptr){
            (*nodes_ptr_)[idx] = new Node(start_x, start_y,theta_idx, start_t,0);
        }

        start_node_ = (*nodes_ptr_)[idx];
        start_node_->sm_state = 0;
        start_node_->t = 0;

        //目标节点
        theta_idx = vehicle_->getCloseTheta(goal.th);
        idx = toIndex(goal_x, goal_y, theta_idx, 0,0);
        if((*nodes_ptr_)[idx] == nullptr){
            (*nodes_ptr_)[idx] = new Node(goal_x, goal_y, theta_idx, 0,0);
        }
        goal_node_ = (*nodes_ptr_)[idx];

        collide_times_ = 0;
        calculate_millisec = 0;
        times_.clear();
        for(int i=0;i<40;i++)
            times_.push_back(0);

        moying_map_->set_path_planing_vehicle_id(vehicle_->getID());
        no_pass_nodes_.clear();
        plan_results_.clear();
        path_plan_start_time_ = std::chrono::system_clock::now();

        NodePtr old_start_node  = start_node_;
        NodePtr old_goal_node   = goal_node_;
        VehicleState start_state,goal_state;
        start_state = start_state_ = start;
        goal_state = goal_state_ = goal;

        //建立局部的代价地图
        local_cost_map_->buildLocalCostMap(vehicle_);

        int max_path_count = 2; //最多规划出的路径数量
        //核心路径规划逻辑
        for(int i=0;i<max_path_count;i++){
            
            start_node_     = old_start_node;
            goal_node_      = old_goal_node;
            start_state_    = start_state;
            goal_state_     = goal_state;
START_REC_TIME
auto temp_time = start_time; 
            bool no_path_2d = false;
            if(!searchPath2D(start,goal,result,no_path_2d,true)){

start_time = temp_time;
END_REC_TIME(15)
                if(no_path_2d){//2d地图规划不出路径，不用再尝试
                    break;
                }
                continue;
            }else{

start_time = temp_time;
END_REC_TIME(15)

                plan_results_.emplace_back(result);

                //检查是否有路径点等待时间过久,如果有尝试规划其他路径
                int start_index,stop_index;
                Node last_node;
                Node max_wait_node;
                int max_wait_time = 0;
                for(int i=0;i<result.node_path.size();i++){
                    auto cur_node = result.node_path[i];
                    if(i == 0){
                        last_node = cur_node;
                        start_index = i;
                        continue;
                    }
                    if(cur_node.x != last_node.x || cur_node.y != last_node.y || i == result.node_path.size()-1){
                        stop_index = i;
                        int diff = result.node_path[stop_index].t - result.node_path[start_index].t;
                        if(diff > max_wait_time){
                             max_wait_time = diff;
                             max_wait_node = last_node;
                        }
                        last_node = cur_node;
                        start_index = i;
                    }
                }
                break;
            }
        }

        moying_map_->set_path_planing_vehicle_id(0);
        HIGHLIGHT("Search time "<<std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - path_plan_start_time_).count());
        INFO("max t is "<<max_t);

        int path_index = -1;
        std::chrono::system_clock::time_point earliest_arrive_time;
        for(int i=0;i<plan_results_.size();i++){
            auto arrive_time = plan_results_[i].path.back().t;
            if(path_index == -1 || earliest_arrive_time > arrive_time){
                path_index = i;
                earliest_arrive_time = arrive_time;
            }
        }

        if(path_index != -1){
            result = plan_results_[path_index];

            if(result.node_path.front().t == 0){    //去掉当前点
                result.node_path.erase(result.node_path.begin());
                result.path.erase(result.path.begin());
            }
            
            vehicle_->set_last_path_plan_result(1);
            return true;
        }else
        {
            vehicle_->set_last_path_plan_result(4);
            WARNING(" times1:"<<times_[0]<<" times2:"<<times_[1]<<" times3:"<<times_[2]<<" times4:"<<times_[3]<<" times5:"<<times_[4]);
            return false;
        }
        
    }

    bool TimeBasedAStar::searchPath2D(const VehicleState &start, const VehicleState &goal, PlanningResult &result,bool& no_path_2d,bool recursion){

        // WARNING("============begin searchPath2D");

        result.start_time = std::chrono::system_clock::now();
        dimension_ = moying_map_->getDimensions();
        dimension_.time = 0;
        nodes_ptr_  = &nodes_2d_;
        closed_ptr_ = &closed_2d_;

        //先保存全局的起始点和目标点
        NodePtr global_start_node   = start_node_;
        NodePtr global_goal_node    = goal_node_;
        VehicleState global_start_state = start_state_;
        VehicleState global_goal_state  = goal_state_;

        std::vector<Node> node_path;
        bool ret = astarSearch(node_path);

        if(ret){
            
START_REC_TIME
            //从node路径生成导航路径
            vehicle_->constructPlanFromGraphPath(moying_map_, start, goal, node_path, result);
            NodePtr conflict_node = nullptr;
            bool find_other_path = false;
END_REC_TIME(16)

            // WARNING("print 2d path, robot_id:"<<vehicle_->getID());
            // displayPath(result);

            std::chrono::system_clock::time_point local_start_time;

            Node last_conflict_node;
            last_conflict_node.x = -1;
            bool contain_local_path = false;
            bool retreat_first_node = false;

            while(true){

                start_node_     = global_start_node;
                goal_node_      = global_goal_node;
                start_state_    = global_start_state;
                goal_state_     = global_goal_state;

                if(!checkPathConflictWithTimemap(result.node_path,conflict_node)){    
                    if(last_conflict_node.x != -1){
                        conflict_node = &last_conflict_node;
                    }
                    break;    //时间无冲突
                }

                //如果离车当前位置很近的目标点有时间冲突,直接返回失败
                if(abs(global_goal_node->x - global_start_node->x) <= 1 
                    && abs(global_goal_node->y - global_start_node->y) <= 1){
                    ret = false;
                    break;
                }

                if(conflict_node == nullptr)
                    return false;
                if(last_conflict_node.isEqual(*conflict_node)){
                    // WARNING("node conflict twice,can't resolve conflict");
                    ret = false;
                    break;
                }

                // WARNING("checked time conflict, x:"<<conflict_node->x<<" y:"<<conflict_node->y<<" theta:"<<(int)conflict_node->th
                //     <<" sm:"<<(int)conflict_node->sm_state << " t:"<<conflict_node->t<<" robot_id:"<<vehicle_->getID());
                // WARNING("last conflict, x:"<<last_conflict_node.x<<" y:"<<last_conflict_node.y<<" theta:"<<(int)last_conflict_node.th
                //     <<" sm:"<<(int)last_conflict_node.sm_state << " t:"<<last_conflict_node.t<<" robot_id:"<<vehicle_->getID());
                
                if(conflict_node != nullptr)
                    last_conflict_node = *conflict_node;

                //准备局部路径规划的环境
                if(!prepareLocal3DMap(result,conflict_node,local_map_,local_start_time)){
                    return false;
                }

                // WARNING("local 3d map prepared, minx:"<<local_map_.min_cell_x<<" miny:"<<local_map_.min_cell_y<<" maxx:"
                //     <<local_map_.max_cell_x<<" maxy:"<<local_map_.max_cell_y<<" mint:"<<local_map_.min_time<<" maxt:"<<local_map_.max_time);

                Node s_node,g_node;
                localNodeToGlobalNode(local_map_,*start_node_,s_node);
                localNodeToGlobalNode(local_map_,*goal_node_,g_node);
                // WARNING("local start node x:"<<s_node.x<<" y:"<<s_node.y<<" theta:"<<(int)s_node.th<<" sm:"<<(int)s_node.sm_state
                //     <<" start_state_x:"<<start_state_.x<<" start_state_y:"<<start_state_.y<<" start_state_th:"<<start_state_.th);
                // WARNING("local goal node x:"<<g_node.x<<" y:"<<g_node.y<<" theta:"<<(int)g_node.th<<" sm:"<<(int)g_node.sm_state
                //     <<" goal_state_x:"<<goal_state_.x<<" goal_state_y:"<<goal_state_.y<<" goal_state_th:"<<goal_state_.th);

                NodePtr local_start_node  = start_node_;
                NodePtr local_goal_node   = goal_node_;
                std::vector<Node> local_path,local_2_global_path;
                PlanningResult local_result;
                ret = astarSearch(local_path,true); //局部路径规划
                // WARNING("local path plan max_t:"<<max_t);
                if(ret){

                    conflict_node = nullptr;

                    // WARNING("local path plan succeed");
                    /***********把局部地图node映射成全局地图node,并生成局部导航路径**********/
                    for(auto node : local_path){
                        Node global_node;
                        if(!localNodeToGlobalNode(local_map_,node,global_node))
                            return false;
                        local_result.node_path.emplace_back(global_node);
                        // local_2_global_path.emplace_back(global_node);
                    }

                    // for(auto node : local_2_global_path){
                    //     HIGHLIGHT("robot_id:"<<vehicle_->getID()<<" x:"<<node.x<<" y:"<<node.y<<" th:"<<(int)node.th
                    //         <<" sm:"<<(int)node.sm_state<<" t:"<<node.t);
                    // }

                    // local_result.start_time = local_start_time;
                    // vehicle_->constructPlanFromGraphPath(moying_map_, local_map_.start, local_map_.goal, local_2_global_path, local_result);
                    /*********************/

                    // WARNING("update t");
                    if(local_result.node_path.size() == 0){
                        WARNING("local path size is zero");
                        return false;
                    }

                    // displayPath(local_result);

                    //用局部路径替换掉原先对应的全局路径部分
                    if(!replaceLocalPath(result,local_result,local_start_node,local_goal_node,local_map_))
                        return false;

                    // WARNING("replace local path");
                    // displayPath(result);
                    contain_local_path = true;
                }
                else{

                    // WARNING("local path plan failed");
                    if(retreat_first_node){
                        // WARNING("retreated to the first node,can't retreat more");
                        break;
                    }
                    if(vehicle_->last_path_plan_result() == 3)//路径规划超时
                        return false;

                    Node global_node;
                    if(!localNodeToGlobalNode(local_map_,*local_start_node,global_node))
                        return false;
                
                    // WARNING("retreat node x:"<<global_node.x<<" y:"<<global_node.y<<" theta:"<<(int)global_node.th
                    //     <<" sm:"<<(int)global_node.sm_state);
                    
                    int retreat_time = local_map_.max_time-local_map_.min_time+1;
                    if(!retreatPathNodeArriveTime(result,global_node,retreat_time,retreat_first_node)){
                        find_other_path = true;
                        break;
                    }
                }

            }
            
            if(conflict_node != nullptr){
                //把冲突点置为临时不可通行点,如果本次规划出的路径在某处等待时间过久,可以搜索是否有其他可通行路径
                std::map<int, FootprintCell> footprint_cells_conflict,footprint_cells_cur;
                vehicle_->getFootprintOccupiedCells(costmap_, conflict_node->x, conflict_node->y, footprint_cells_conflict);
                vehicle_->getFootprintOccupiedCells(costmap_, global_start_node->x, global_start_node->y, footprint_cells_cur);

                for(auto item : footprint_cells_conflict){
                    if(!item.second.inflate)
                        no_pass_nodes_.emplace_back(item.second);
                    // WARNING("can't pass temporarily node x:"<<item.second.x<<" y:"<<item.second.y);
                }
                if(find_other_path){
                    result.clear();
                    start_node_     = global_start_node;
                    goal_node_      = global_goal_node;
                    start_state_    = global_start_state;
                    goal_state_     = global_goal_state;
                    if(!recursion)
                        return false;
                    return searchPath2D(start,goal,result,no_path_2d,false);
                }
            }

            //如果局部规划了路径,重新构造一下整条路径
            if(contain_local_path){
                node_path.clear();
                node_path = result.node_path;
                vehicle_->constructPlanFromGraphPath(moying_map_, start, goal, node_path, result);
            }
            
        }else{
            no_path_2d = true;
        }

START_REC_TIME
        if(ret){
            // bool collide = false;
            // //路径规划成功后，使用导航坐标检查是否会和局部障碍物发生碰撞，如果发生碰撞，把碰撞点机器人足迹加入到临时不可通行列表

            // sch::ConfigInfo& cfg_info = sch::ScheduleConfig::instance()->getConfigInfo();
            // float l_padding = cfg_info.traffic_manager_cfg.robot_length_padding;
            // float w_padding = cfg_info.traffic_manager_cfg.robot_width_padding;

            // for (int i=0;i< result.path.size();i++){
            //     auto path = result.path[i];
            //     auto node = result.node_path[i];
            //     if(moying_map_->checkCollideLocalObstacle(vehicle_->getID(),path.x,path.y,path.th,nullptr,l_padding,w_padding)){
            //         std::map<int, FootprintCell> footprint_cells;
            //         // vehicle_->getFootprintOccupiedCells(costmap_, result.node_path[i].x,  result.node_path[i].y, footprint_cells);
            //         // for(auto item : footprint_cells){
            //         //     no_pass_nodes_.emplace_back(item.second);
            //         // }
            //         FootprintCell fpc;
            //         fpc.x = node.x;
            //         fpc.y = node.y;
            //         fpc.inflate = false;
            //         no_pass_nodes_.emplace_back(fpc);

            //         collide = true;
            //         WARNING("check collision, try path replan,collide_x:"<<node.x<<" collide_y:"<<node.y
            //             <<" collide_th:"<<(int)node.th);
            //     }

            // }

            // if(collide){    //发生碰撞，重新规划
            //     result.clear();
            //     start_node_     = global_start_node;
            //     goal_node_      = global_goal_node;
            //     start_state_    = global_start_state;
            //     goal_state_     = global_goal_state;
            //     if(!recursion)
            //         return false;
            //     return searchPath2D(start,goal,result,false);
            // }
           
        }
END_REC_TIME(17)

        return ret;
    }

    bool TimeBasedAStar::checkPathConflictWithTimemap(std::vector<Node>& node_path,NodePtr& conflict_node){

        if(node_path.empty())
            return false;


        bool is_free = true;
        NodePtr inplace_node = &node_path[0];
        for(int i=0;i<node_path.size();i++){
            NodePtr cur_node = nullptr;
            NodePtr next_node = nullptr;
            if(i == 0){
                cur_node = &node_path[0];
                next_node = &node_path[0];
            }else{
                cur_node = &node_path[i-1];
                next_node = &node_path[i];
            }

            if(cur_node->x == inplace_node->x && cur_node->y == inplace_node->y)   //机器人当前位置时间不冲突 
                continue;

            is_free = moying_map_->isFree(vehicle_,cur_node,next_node);

            if(!is_free){

                conflict_node = &node_path[i];
#if defined(__DEBUG__)
                WARNING("===== time conflict in cur_x:"<<cur_node->x<<" cur_y:"<<cur_node->y<<" next_x:"<<next_node->x<<" next_y:"<<next_node->y);
                auto time_map = moying_map_->timed_map();
                TimeOccupationListPtr list_ptr_a = (*time_map)[cur_node->x][cur_node->y];
                TimeOccupationListPtr list_ptr_b = (*time_map)[next_node->x][next_node->y];
                for(int t = cur_node->t;t<=next_node->t;t++){
                    if(list_ptr_a && list_ptr_a->at(t) != nullptr){
                        if(list_ptr_a->at(t)->getName().compare("VehicleInflation") == 0)
                        {
                            VehicleInflationPtr vehicle_inflation = std::static_pointer_cast<VehicleInflation>(list_ptr_a->at(t));
                            if(vehicle_inflation){
                                printf(" cur_node t:%d ",t);
                                vehicle_inflation->showVehicles();
                            }
                        }
                    }

                    if(list_ptr_b->at(t) != nullptr){
                        if(list_ptr_b && list_ptr_b->at(t)->getName().compare("VehicleInflation") == 0)
                        {
                            VehicleInflationPtr vehicle_inflation = std::static_pointer_cast<VehicleInflation>(list_ptr_b->at(t));
                            if(vehicle_inflation){
                                printf(" next_node t:%d ",t);
                                vehicle_inflation->showVehicles();
                            }
                            
                        }
                    }
                }
#endif
                return true;
            }
        }
        return false;
    }

    bool TimeBasedAStar::replaceLocalPath(PlanningResult &global_result,PlanningResult &local_result,NodePtr local_start_node,
                NodePtr local_goal_node,Local3DMap local_map){

        Node global_start_node;    //局部地图的起始节点映射到全局地图中的起始节点
        Node global_goal_node;     //局部地图的目标节点映射到全局地图中的目标节点
        bool ret = localNodeToGlobalNode(local_map,*local_start_node,global_start_node);
        ret = ret & localNodeToGlobalNode(local_map,*local_goal_node,global_goal_node);
        if(!ret)
            return false;

        // std::vector<VehicleState> path_vec;
        std::vector<Node> node_vec;
        
        //copy全局路径中进入局部地图之前的节点
        for(int i=0;i< global_result.node_path.size();i++){
            auto node = global_result.node_path[i];
            node_vec.emplace_back(node);

            if(node.isEqual(global_start_node) ){   //局部路径里已经有该起始点
                node_vec.pop_back();
                break;
            }
        }

        //copy局部地图里的节点
        for(int i=0;i<local_result.node_path.size();i++){
            auto node = local_result.node_path[i];
            node.t += local_map_.min_time;
            node_vec.emplace_back(node);
        }

        //copy全局路径离开局部地图后的节点
        bool begin_copy = false;
        bool arrive_goal_node = false;
        int start_t = node_vec.back().t;
        int old_start_t;
        for(int i=0;i< global_result.node_path.size();i++){
            auto node = global_result.node_path[i];
            if(!begin_copy){
                if(!arrive_goal_node){
                    if(node.isEqual(global_goal_node))
                        arrive_goal_node = true;
                    continue;
                }
                
                //如果全局路径中在局部目标点node有等待,把这些等待node忽略掉
                if(arrive_goal_node && node.isEqual(global_goal_node)){
                    continue;
                }else if(arrive_goal_node && !node.isEqual(global_goal_node)){
                    begin_copy = true;
                    old_start_t = global_result.node_path[i-1].t;
                }

            }

            node.t = start_t + node.t - old_start_t;
            node_vec.emplace_back(node);
        }

        global_result.clear();
        for(int i=0;i<node_vec.size();i++){
            global_result.node_path.push_back(node_vec[i]);
        }

        return true;
    }

    bool TimeBasedAStar::retreatPathNodeArriveTime(PlanningResult& global_result,const Node& retreat_node,int retreat_time,bool& retreat_first_node){
        int i=0;
        for(;i<global_result.node_path.size();i++){
            auto node = global_result.node_path[i];
            if(node.isEqual(retreat_node)){
                break;
            }
        }
        if(i == 0){  //没有节点可回退
            WARNING("no node can retreat");
            return false;
        }
        i -= retreat_time;
        i = i < 0 ? 0 : i;
        retreat_first_node = i == 0;

        //在回退点等待一段时间
        int wait_time = 5;
        float time_resolution = moying_map_->time_resolution();
        if(time_resolution != 0)
            wait_time /= time_resolution;

        std::vector<Node> node_vec;
        std::vector<VehicleState> path_vec;

        for(int n=0;n<global_result.node_path.size();n++){

            auto node = global_result.node_path[n];
            auto state= global_result.path[n];

            if(n < i ){
                node_vec.push_back(global_result.node_path[n]);
                path_vec.push_back(global_result.path[n]);
                continue;
            }else if(n == i){//添加等待节点
                node_vec.push_back(node);
                path_vec.push_back(state);
                MovePattern mp(0,0,0,0,0,kStaticState);
                mp = Vehicle::getMovePattern(MovePatternType::kWaitPattern);
                for(int t=0;t<wait_time;t++){
                    node.t ++;
                    state.t = global_result.start_time + std::chrono::milliseconds( 1000*node.t );
                    node_vec.push_back(node);
                    path_vec.push_back(state);
                }
            }else{
                node.t += wait_time;
                state.t = global_result.start_time + std::chrono::milliseconds( 1000*node.t );
                node_vec.push_back(node);
                path_vec.push_back(state);
            }
            
        }

        global_result.clear();
        global_result.node_path = node_vec;
        global_result.path  = path_vec;
        return true;
    }

    bool TimeBasedAStar::localNodeToGlobalNode(const Local3DMap& local_map,const Node& local_node,Node& global_node){


        int global_x = local_node.x + local_map.min_cell_x;
        int global_y = local_node.y + local_map.min_cell_y;

        MapDimension d = dimension_;
        dimension_ = moying_map_->getDimensions();
        dimension_.time = 0;
        int idx = toIndex(global_x,global_y,local_node.th,0,local_node.sm_state);
        if(idx >= global_node_size_ || idx < 0){
            WARNING("idx of nodes is invalid, idx:"<<idx);
            dimension_ = d;
            return false;
        }
        if(nodes_2d_[idx] == nullptr){
            nodes_2d_[idx] = new Node(global_x,global_y,local_node.th,0,local_node.sm_state);
        }
        global_node = *nodes_2d_[idx];
        global_node.t = local_node.t;
        dimension_ = d;
        return true;
    }

    bool TimeBasedAStar::prepareLocal3DMap(const PlanningResult& global_result, const NodePtr& conflict_node,Local3DMap& local_map,
            std::chrono::system_clock::time_point& start_time){

        float x_size = cfg_info_.traffic_manager_cfg.local_path_plan_width;
        float y_size = cfg_info_.traffic_manager_cfg.local_path_plan_width;
        int time_size = cfg_info_.traffic_manager_cfg.local_path_plan_time_size;
        float time_resolution = moying_map_->time_resolution();
        if(time_resolution != 0)
            time_size /= time_resolution;

        int cell_x_size = ceil(x_size / moying_map_->getCostMap()->getResolution());
        int cell_y_size = ceil(y_size / moying_map_->getCostMap()->getResolution());
        MapDimension d = moying_map_->getDimensions();
        int max_x_size = d.x;
        int max_y_size = d.y;
        int max_t_size = d.time;
        const std::vector<Node>& global_node_path = global_result.node_path;
        NodePtr global_start_node   = start_node_;
        NodePtr global_goal_node    = goal_node_;

        dimension_.x = cell_x_size;
        dimension_.y = cell_y_size;
        dimension_.time = time_size;

        if(local_node_size_ == 0){
            local_node_size_ = dimension_.x*dimension_.y*dimension_.theta*dimension_.sm_state*dimension_.time;
            // nodes_3d_.resize(node_size);
            nodes_3d_ = new NodePtr[local_node_size_];
            memset(nodes_3d_,0,local_node_size_*sizeof(NodePtr));
            closed_3d_.resize(local_node_size_);
        }
        nodes_ptr_ = &nodes_3d_;
        closed_ptr_ = &closed_3d_;

        //计算局部地图在整个地图中的边界
        int minx,miny,maxx,maxy,mint,maxt;
        minx = conflict_node->x - cell_x_size/2;
        maxx = minx+cell_x_size-1;
        miny = conflict_node->y - cell_y_size/2;
        maxy = miny+cell_y_size-1;
        if(minx < 0){
            minx = 0;
            maxx = cell_x_size-1;
        }else if(maxx >= max_x_size){
            maxx = max_x_size-1;
            minx = maxx -cell_x_size+1;
        }
        if(miny < 0){
            miny = 0;
            maxy = cell_y_size-1;
        }else if(maxy >= max_y_size){
            maxy = max_y_size-1;
            miny = maxy -cell_y_size+1;
        }
        local_map.min_cell_x    = minx;
        local_map.min_cell_y    = miny;
        local_map.max_cell_x    = maxx;
        local_map.max_cell_y    = maxy;

        mint = conflict_node->t - time_size/2;
        maxt = mint + time_size-1;
        if(mint < 0){
            mint = 0;
            maxt = time_size-1;
        }else if(maxt >= max_t_size){
            maxt = max_t_size-1;
            time_size = maxt - mint+1;
        }
        dimension_.time = time_size;
        local_map.min_time = mint;
        local_map.max_time = maxt;

        //找出局部地图中的起始点和终止点
        bool start_node_find = false;
        bool goal_node_find = false;
        bool conflict_passed = false;//寻找局部地图里的目标点时,要必须经过冲突点
        for(int i=0;i<global_node_path.size();i++){

            auto node = global_node_path[i];
            int x = node.x;
            int y = node.y;
            int t = node.t;
            bool within = false;
            if(x >= local_map.min_cell_x && x <= local_map.max_cell_x 
                && y >= local_map.min_cell_y && y <= local_map.max_cell_y
                && t >= mint && t <= maxt){
                within = true;
            }

            if(x == conflict_node->x && y == conflict_node->y){
                conflict_passed = true;
            }

            if(!start_node_find && within){

                /**********根据局部地图路径规划起点,重新确定时间维度的最大最小值***********/
                mint = node.t;
                maxt = mint + time_size-1;
                if(mint < 0){
                    mint = 0;
                    maxt = time_size-1;
                }else if(maxt >= max_t_size){
                    maxt = max_t_size-1;
                    time_size = maxt - mint+1;
                }
                dimension_.time = time_size;
                local_map.min_time = mint;
                local_map.max_time = maxt;
                /*****************************/

                int cx = x-local_map.min_cell_x;
                int cy = y-local_map.min_cell_y;
                int th = node.th;
                int sm = node.sm_state;
                int indx = toIndex(cx,cy,th,0,sm);
                if(indx >= local_node_size_)
                    break;
                if(nodes_3d_[indx] == nullptr){
                    nodes_3d_[indx] = new Node(cx, cy, th, 0,sm);
                }
                double wx,wy;
                // moying_map_->getCostMap()->mapToWorld(x,y,wx,wy);
                if( !moying_map_->getCellNavCoor(x,y,wx,wy,vehicle_) )
                    return false;
                local_map.start.x = wx;
                local_map.start.y = wy;
                local_map.start.th = vehicle_->getTheta(node.th);
                start_node_ = nodes_3d_[indx];
                start_node_->t = 0;
                start_time = global_result.path[i].t;
                start_node_find = true;

            }else if(start_node_find && (!within || i == global_node_path.size()-1) && conflict_passed){

                int index = i-1;
                if(i == global_node_path.size()-1)
                    index = i;
                if(index < 0)
                    break;
                node = global_node_path[index];
                int cx = node.x-local_map.min_cell_x;
                int cy = node.y-local_map.min_cell_y;
                int th = node.th;
                int sm = node.sm_state;
                int indx = toIndex(cx,cy,th,0,sm);
                if(nodes_3d_[indx] == nullptr){
                    nodes_3d_[indx] = new Node(cx, cy, th, 0,sm);
                }
                double wx,wy;
                // moying_map_->getCostMap()->mapToWorld(node.x,node.y,wx,wy);
                if( !moying_map_->getCellNavCoor(node.x,node.y,wx,wy,vehicle_))
                    return false;
                local_map.goal.x = wx;
                local_map.goal.y = wy;
                local_map.goal.th = vehicle_->getTheta(node.th);
                goal_node_ = nodes_3d_[indx];
                goal_node_find = true;

                break;
            }
        }

        if(!start_node_find || !goal_node_find)
            return false;
        else
        {
            Node g_start,g_goal;
            localNodeToGlobalNode(local_map,*start_node_,g_start);
            localNodeToGlobalNode(local_map,*goal_node_,g_goal);
            if(!g_start.isEqual(*global_start_node)){
                // moying_map_->getCostMap()->mapToWorld(g_start.x,g_start.y,start_state_.x,start_state_.y);
                if( !moying_map_->getCellNavCoor(g_start.x,g_start.y,start_state_.x,start_state_.y,vehicle_) ){
                    return false;
                }
                start_state_.th = vehicle_->getTheta(g_start.th);
            }
            if(!g_goal.isEqual(*global_goal_node)){
                // moying_map_->getCostMap()->mapToWorld(g_goal.x,g_goal.y,goal_state_.x,goal_state_.y);
                if( !moying_map_->getCellNavCoor(g_goal.x,g_goal.y,goal_state_.x,goal_state_.y,vehicle_) ){
                    return false;
                }
                goal_state_.th = vehicle_->getTheta(g_goal.th);
            }
            return true;
        }
        
    }

    bool TimeBasedAStar::handleStartCell(std::vector<Node> &node_path){

        std::vector<NodePtr> neighbour_nodes;
        std::pair<int,int> adds[] = 
                                    {
                                        {-1,1},{0,1},{1,1},
                                        {-1,0},{1,0},
                                        {-1,-1},{0,-1},{1,-1}
                                    };
        
        /*************先检查当前点或临近点是否是终点,如果是,直接走向终点***********/
        bool direct_to_node = false;
        if(goal_node_->x == start_node_->x && goal_node_->y == start_node_->y){
            direct_to_node = true;
        }else{
            for(int i=0;i<8;i++){
                int x = start_node_->x+adds[i].first;
                int y = start_node_->y+adds[i].second;
                
                if(goal_node_->x == x && goal_node_->y == y){
                    direct_to_node = true;
                    break;
                }
            }
        }
        if(direct_to_node){

            std::vector<Node> node_vec;
            bool ret = getMoveToNeighbourPath(start_state_,goal_state_,start_node_,goal_node_,node_vec);
            if(!ret || node_vec.empty())
                return false;
            node_path.emplace_back(*start_node_);
            for(int i=0;i<node_vec.size();i++){
                auto node = node_vec[i];
                int n = toIndex(node.x,node.y,node.th,node.t,node.sm_state);
                (*closed_ptr_)[n] = true;
                if(i == node_vec.size()-1){
                    open_.emplace(0,(*nodes_ptr_)[n]);
                }
                // printf("=====start node x:%d y:%d th:%d sm:%d t:%d \n",node.x,node.y,node.th,node.sm_state,node.t);
                node_path.emplace_back(node);
            }
            
            return true;
        }
        /************************/

        int search_theta_num = cfg_info_.traffic_manager_cfg.search_theta_num;

        for(int i=0;i<8;i++){//for1
            int x = start_node_->x+adds[i].first;
            int y = start_node_->y+adds[i].second;
            for(int th=0;th<8;th++){//for2

                if(search_theta_num == 4 && (th == 1 || th == 3 || th == 5 || th == 7)){
                    continue;
                }
                for(int sm=0;sm<2;sm++){//for3
                    Node node = Node(x, y, th, 0,sm);
                    NodePtr next_node = &node;
                    VehicleState next_state;
                    next_state.th = vehicle_->getTheta(next_node->th);
                    if(local_path_plan_){
                        Node g_node;
                        if( !localNodeToGlobalNode(local_map_,node,g_node) )
                            return false;
                        if( !moying_map_->getCellNavCoor(g_node.x,g_node.y,next_state.x,next_state.y,vehicle_) )
                            return false;
                    }else{
                        if( !moying_map_->getCellNavCoor(next_node->x,next_node->y,next_state.x,next_state.y,vehicle_) )
                            return false;
                    }
                    
                    std::vector<Node> node_vec;
                    bool ret = getMoveToNeighbourPath(start_state_,next_state,start_node_,next_node,node_vec);
                    if(!ret){
                        // if(local_path_plan_){
                        //     WARNING("local path plan get start node failed\n");
                        // }
                        continue;
                    }
                    start_node_->t = 0;
                    if(node_vec.size()>0){
                        // printf("begin=============\n");
                    }
                    for(int i=0;i<node_vec.size();i++){//for4
                        auto node = node_vec[i];
                        int n = toIndex(node.x,node.y,node.th,node.t,node.sm_state);
                        NodePtr& p_node = (*nodes_ptr_)[n];
                        if( p_node == nullptr){
                            p_node = new Node(node.x,node.y,node.th,node.t,node.sm_state);
                        }
                        (*closed_ptr_)[n] = true;
                        if(i == node_vec.size()-1){
                            *p_node = node;
                            p_node->parent = nullptr;
                            open_.emplace(node.f,p_node);
                        }
                        // printf("=====start node x:%d y:%d th:%d sm:%d t:%d parent:%p\n",node.x,node.y,node.th,node.sm_state,node.t,node.parent);
                    }//for4
                }//for3
            }//for2
        }//for1

        return true;
    }

    bool TimeBasedAStar::handleGoalCell(const NodePtr current,const NodePtr next_node){

        NodePtr temp = current;

        VehicleState cur_state,next_state;
        if(current->parent == nullptr){
            cur_state = start_state_;
        }else{

            if(local_path_plan_){
                Node g_node;
                if( !localNodeToGlobalNode(local_map_,*current,g_node) )
                    return false;
                if( !moying_map_->getCellNavCoor(g_node.x,g_node.y,cur_state.x,cur_state.y,vehicle_) )
                    return false;
            }else{
                if( !moying_map_->getCellNavCoor(current->x,current->y,cur_state.x,cur_state.y,vehicle_) )
                    return false;
            }
            cur_state.th = vehicle_->getTheta(current->th);
        }
        next_state = goal_state_;
        std::vector<Node> node_vec;
        Node last_current = *current;   //先保存当前节点,防止在getMoveToNeighbourPath内部被修改数据
        bool ret = getMoveToNeighbourPath(cur_state,next_state,current,next_node,node_vec,false);
        if(!ret){
            printf("handleGoalCell failed cur_x:%d,cur_y:%d,cur_th:%d,cur_sm:%d,next_x:%d,next_y:%d,next_th:%d,next_sm:%d,robot_id:%d\n",
                current->x,current->y,(int)current->th,
                    (int)current->sm_state,next_node->x,next_node->y,(int)next_node->th,(int)next_node->sm_state,vehicle_->getID());
        }
        return ret;

    }

    bool TimeBasedAStar::astarSearch(std::vector<Node> &node_path,bool local_path_plan)
    {
        calculate_millisec = 0;
        resetNodes();
        local_path_plan_ = local_path_plan;
        start_node_->g = 0;
        start_node_->f = heuristic(start_node_, goal_node_);
        start_node_->parent = nullptr;
        // open_.emplace(start_node_->f, start_node_);
        unsigned int idx = toIndex(start_node_->x, start_node_->y, start_node_->th, 0,start_node_->sm_state);
        (*closed_ptr_)[idx] = true;

        node_path.clear();

        open_.emplace(start_node_->f, start_node_);
        node_path.emplace_back(*start_node_);


        NodePtr current;
        std::multimap<unsigned int, NodePtr>::iterator it;

        while(!open_.empty())
        {   
            it = open_.begin();
            current = it->second;
            open_.erase(it);

START_REC_TIME
auto time_begin = start_time;
            bool find_dest=false;
            addValidNeighbors(current,find_dest);
start_time = time_begin;
END_REC_TIME(13)

            if(find_dest){
                HIGHLIGHT("Node path found and constructed, closedset size "<<closed_ptr_->count()<<" left over openset size "<<open_.size()
                <<" robot_id:"<<vehicle_->getID());
                break;
            }

            auto elapse_time =  std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - path_plan_start_time_).count();
            if(elapse_time > cfg_info_.traffic_manager_cfg.max_path_plan_milliseconds){
                WARNING("path plan time is too long, stop path plan, elapse time:"<<elapse_time<<" milliseconds ,robot_id:"<<vehicle_->getID()<<
                " closedset size "<<closed_ptr_->count()<<" left over openset size "<<open_.size());
                vehicle_->set_last_path_plan_result(3);
                return false;
            }
        }

        if(path_end_nodes_.size()>0){

            PathEndNodeInfo info = path_end_nodes_.begin()->second;
            if(!info.nodes.empty()){
                node_path.clear();

                //添加特殊处理的尾部节点
                for(int i=info.nodes.size()-1;i>=0;i--){
                    node_path.emplace_back(info.nodes[i]);
                }

                //如果上一个节点是旋转动作,且本节点也是旋转动作,上上一个节点朝向直接旋转到本节点朝向 
                if(info.parent != nullptr && info.parent->parent != nullptr){
                    auto node1 = info.nodes.front();
                    auto node2 = node1.parent;
                    auto node3 = node2->parent;

                    bool rotate = node2->th != node3->th || node2->theta != node3->theta;
                    if(node1.x == node2->x && 
                    node2->x == node3->x &&  
                    node1.y == node2->y &&
                    node2->y == node3->y &&
                    rotate  ){
                        //如果尾部特殊处理前的节点是旋转,且尾部特殊特殊处理的第一个节点也是旋转,则只旋转一次.
                        info.parent = info.parent->parent;
                    }
                }
                

                NodePtr temp = info.parent;
                //添加中间节点
                while (temp != nullptr )
                {
                    node_path.emplace_back(*temp);
                    if(temp->parent == nullptr)
                        break;
                    temp = temp->parent;
                }

                WARNING("path_start_nodes_.size:"<<path_start_nodes_.size());
                //添加头部节点
                for(auto iter=path_start_nodes_.begin();iter!=path_start_nodes_.end();iter++){
                    auto item = *iter;
                    auto nodes  = item.second.nodes;
                    Node node   = nodes.back();
                    // printf("x:%d,y:%d,th:%d,t:%d,sm:%d\n",node.x, node.y, (int)node.th, node.t,(int)node.sm_state);
                    unsigned int idx = toIndex(node.x, node.y, node.th, node.t,node.sm_state);
                    if((*nodes_ptr_)[idx] == temp && temp != nullptr){
                        WARNING("start nodes size:"<<nodes.size());
                        
                        for(int i=nodes.size()-2;i>=0;i--){ //头部节点数组中的最后一个元素被添加过,不要重复添加
                            node_path.emplace_back(nodes[i]);
                        }
                        if(item.second.parent != nullptr)
                            node_path.emplace_back(*item.second.parent);   //把起始节点添加进去
                        break;
                    }
                }
    // WARNING("end contruct path list");
                std::reverse(node_path.begin(), node_path.end());
                return true;
            }
        }

        WARNING("A star failed to find solution, closedset size "<<closed_ptr_->count()<<" left over openset size "<<open_.size()
                    <<" robot_id:"<<vehicle_->getID() << " collide_times:"<<collide_times_ <<" calculate collide time:"<<calculate_millisec);
        return false;
            
    }

    int TimeBasedAStar::heuristic(const NodePtr &a, const NodePtr &b)
    {
        int dx = abs(a->x - b->x);
        int dy = abs(a->y - b->y);
        return (dx + dy)*2;
    }

    void TimeBasedAStar::addValidNeighbors(const NodePtr &current, bool &find_dest)
    {
        MovePatternsPtr move_patterns = nullptr;
        int x,y,th;
        
        if(local_path_plan_){
            Node global_node;
            if(!localNodeToGlobalNode(local_map_,*current,global_node)){
                return;
            }
            x = global_node.x;
            y = global_node.y;
            th = global_node.th;
        }else{
            x = current->x;
            y = current->y;
            th = current->th;
        }

        // 看看是否有定制路线

START_REC_TIME
        int search_theta_num = cfg_info_.traffic_manager_cfg.search_theta_num;
        move_patterns = move_patterns_in_map_->getCustomMove(x, y, th, (MovePatternState)current->sm_state,search_theta_num);
END_REC_TIME(21)

        if(move_patterns != nullptr)
        {

            for(int i = 0; i < move_patterns->size(); i++)
            {
                MovePatternType type = move_patterns->at(i);

                if( !vehicle_->isMovePatternValid(type,(MovePatternState)current->sm_state) )
                    continue;          
                const MovePattern& mp = vehicle_->getMovePattern(type);
                if(dimension_.time == 0 && mp.x == 0 && mp.y == 0 && mp.dir_change == 0){ //二维地图不考虑原地等待
                    continue;
                }

                addValidNeighbor(current, mp, find_dest);
                if(find_dest)
                    return;
            }
        }

    }

    void TimeBasedAStar::addValidNeighbor(const NodePtr &current, const MovePattern& move_pattern, bool &find_dest)
    {   
        
        int x, y, th, tn, idxn;
        MapDimension& d = dimension_;

START_REC_TIME
        switch (current->th)
        {
        case 0:
            x = current->x + move_pattern.x;
            y = current->y + move_pattern.y;
            break;
        case 1:
            x = current->x + move_pattern.x - move_pattern.y;
            y = current->y + move_pattern.x + move_pattern.y;
            break;
        case 2:
            x = current->x - move_pattern.y;
            y = current->y + move_pattern.x;
            break;
        case 3:
            x = current->x - move_pattern.x - move_pattern.y;
            y = current->y + move_pattern.x - move_pattern.y;
            break;
        case 4:
            x = current->x - move_pattern.x;
            y = current->y - move_pattern.y;
            break;
        case 5:
            x = current->x - move_pattern.x + move_pattern.y;
            y = current->y - move_pattern.x - move_pattern.y;
            break;
        case 6:
            x = current->x + move_pattern.y;
            y = current->y - move_pattern.x;
            break;
        case 7:
            x = current->x + move_pattern.x + move_pattern.y;
            y = current->y - move_pattern.x + move_pattern.y;
            break;
        default:
            break;
        }

        th = (current->th + move_pattern.dir_change+d.theta) % d.theta;
        tn = current->t + move_pattern.t;

        if(x < 0 || x >= d.x || y < 0 || y >= d.y || (tn >= d.time && d.time != 0) || th < 0 || th >= d.theta){
END_REC_TIME(0)
            return;
        }
END_REC_TIME(0)

// printf("enter addValidNeighbor,x:%d,y:%d th:%d\n",x,y,th);

START_REC_TIME
        Node node = Node(x, y, th, tn,move_pattern.sm_state);
        calculateNodeCost(current,&node,move_pattern);
END_REC_TIME(3)

START_REC_TIME
        idxn = toIndex(x, y, th, tn,move_pattern.sm_state);
        auto& p_node = (*nodes_ptr_)[idxn];
        if( p_node == nullptr){
            p_node = new Node(x, y, th, tn,move_pattern.sm_state);
        }
END_REC_TIME(2)

START_REC_TIME        
        if( (*closed_ptr_)[idxn] ){


                if(p_node->g > node.g ){
                    *p_node   = node;
                    open_.emplace(p_node->f, p_node);
                }

END_REC_TIME(1)
            
            return;
        }
END_REC_TIME(1)


        // if(vehicle_->isOmniVehicle()){
            if(x == goal_node_->x && y == goal_node_->y && th == goal_node_->th){
                node.t++;
                PathEndNodeInfo node_info;
                node_info.parent = current;
                node_info.nodes.emplace_back(node);
                path_end_nodes_.emplace(0,node_info);
                find_dest = true;
                return;
            }
        // }else{
        //     // //目标节点特殊处理
        //     if(x == goal_node_->x && y == goal_node_->y){
        //         find_dest = handleGoalCell(current,goal_node_);
        //         return;
        //     }
        // }

START_REC_TIME
auto cache_time = start_time;
        if( !isNodeCanPass(current,&node) ){

            // if(current->x == 8 && current->y == 10 && current->th == 6 && node.x == 8 && node.y == 9 && node.th == 6){
            //     printf("8 10 9 6 can't pass\n");
            // }
start_time = cache_time;
END_REC_TIME(4)
            return;
        }

        //规划时不用考虑单行道方向
//         auto all_one_way_road = moying_map_->all_one_way_road();
//         for(auto one_way_road : (*all_one_way_road)){
//             Orientation orient = one_way_road->temp_fix_orientation();
//             if( orient == kUndefine){
//                 continue;
//             }else if( one_way_road->isCellInRoad(MapCell(node.x,node.y)) ){ //如果下一个node在单行道里,且单行道设置了固定临时通行方向,下一个node方向就要做一个匹配 
//                 if(orient != Orientation(node.th) ){
//                     return;
// start_time = cache_time;
// END_REC_TIME(4)
//                 }
//             }
//         }
// start_time = cache_time;
// END_REC_TIME(4)

        //检测单向道是否可通行
#ifdef CHECK_ONE_WAY
        for(auto owr : *moying_map_->all_one_way_road()){
            if(owr->orientation_count() != 1){
                continue;
            }else if(owr->orientation_count() == 1){    
                //TODO check path direction allowed
            }
        }
#endif

START_REC_TIME
        *p_node   = node;
        open_.emplace(p_node->f, p_node);
// if(current->x == 12 && current->y == 56){
//     printf(" --- add cx：%d cy:%d cth:%d csm:%d nx:%d ny:%d nth:%d nsm:%d\n",
//         current->x,current->y,(int)current->th,(int)current->sm_state,x,y,(int)th,(int)move_pattern.sm_state);
// }
END_REC_TIME(5)

START_REC_TIME
        (*closed_ptr_)[idxn] = true;

        if(node.t > max_t)
            max_t = node.t;
END_REC_TIME(6)

    }

    bool TimeBasedAStar::isNodeCanPass(const NodePtr current,const NodePtr neighbor){

// START_REC_TIME
        //如果是2d地图路径规划,检测是否在临时不可通行node里
        if(!local_path_plan_){
            for(auto node : no_pass_nodes_){
                if(neighbor->x == node.x && neighbor->y == node.y){
                    return false;
                }
            }

            //footprint内部的点直接过
            // FootprintOnMap footprint_cells;
            // vehicle_->getFootprintOccupiedCells(costmap_,neighbor->x,neighbor->y,footprint_cells);
            // for(auto item : footprint_cells){
            //     auto c = item.second;
            //     if(!item.second.inflate && neighbor->x == c.x && neighbor->y == c.y){
            //         return true;
            //     }
            // }

        }
// END_REC_TIME(7)

// START_REC_TIME
        Node global_current,global_neighbor;
        if(local_path_plan_){
            if(!localNodeToGlobalNode(local_map_,*current,global_current)){
//END_REC_TIME(8)                
                return false;
            }
            if(!localNodeToGlobalNode(local_map_,*neighbor,global_neighbor)){
//END_REC_TIME(8)
                return false;
            }

        }else{
            global_current = *current;
            global_neighbor = *neighbor;
        }
//END_REC_TIME(8)

//START_REC_TIME
        if(search_cell_area_.valid){
            if(global_neighbor.x < search_cell_area_.min_cell_x || global_neighbor.y < search_cell_area_.min_cell_y ||
                global_neighbor.x > search_cell_area_.max_cell_x || global_neighbor.y > search_cell_area_.max_cell_y){
//END_REC_TIME(9)
                return false;
                }
        }
//END_REC_TIME(9)

//START_REC_TIME
        //检测是否可以旋转
        // if( global_current.x == global_neighbor.x && global_current.y == global_neighbor.y && global_current.th != global_neighbor.th){

        //     if(costmap_->getCost(global_neighbor.x, global_neighbor.y) != MAPCOSTS::FREE){
        //         if( !moying_map_->isCellCanRotate(global_neighbor.x,global_neighbor.y,global_current.th,global_neighbor.th,
        //                 vehicle_->getLength(),vehicle_->getWidth()) ){
        //             return false;
        //         }
        //     }else{
        //         // auto ccs = moying_map_->corrected_cells(vehicle_); 
        //         // if(!ccs->empty()){
        //         //     auto cci = ccs[0];
        //         //     if( !cci[global_current.x][global_current.y].can_rotate 
        //         //         && abs(global_current.th - global_neighbor.th) > 1)   //这里的标志是在寻找单行道时设置的
        //         //         return false;
        //         // }
        //     } 
        // }
        
//END_REC_TIME(10)
        
//START_REC_TIME
        if(neighbor->x != start_node_->x || neighbor->y != start_node_->y){
            if(neighbor->x == goal_node_->x && neighbor->y == goal_node_->y){   //如果是目标点就不要使用cell做碰撞测试了,cell的粒度太大,直接使用激光点
                float theta = neighbor->theta;
                if(theta != 0){
                    theta = neighbor->theta;
                }else{
                    theta = vehicle_->getTheta(neighbor->th);
                }
                moying_map_->checkCollideLocalObstacle(vehicle_->getID(),goal_state_.x,goal_state_.y,theta);

            }else if(moying_map_->checkCollideLocalObstacleCell(vehicle_->getID(),global_neighbor.x,global_neighbor.y,true)){
//END_REC_TIME(11)
                return false;
            }
        }
//END_REC_TIME(11)

//START_REC_TIME
        if(local_path_plan_){   //局部路径规划才检测时间是否冲突
            global_current.t += local_map_.min_time;
            global_neighbor.t += local_map_.min_time;

            //起始node内部不做时间冲突检测 
            bool check_timemap = true;

            if(check_timemap && !moying_map_->isFree(vehicle_, &global_current, &global_neighbor)){
//END_REC_TIME(12)
                return false;
            }
        }
//END_REC_TIME(12)
        return true;
    }


    bool TimeBasedAStar::calculateNodeCost(const NodePtr cur_node,NodePtr next_node,const MovePattern& pattern){

        if(cur_node == nullptr || next_node == nullptr || cur_node == next_node){
            WARNING("calculateNodeCost return false");
            return false;
        }

        int mx,my;
        if(local_path_plan_){
            mx = local_map_.min_cell_x + next_node->x;
            my = local_map_.min_cell_y + next_node->y;
        }else{
            mx = next_node->x;
            my = next_node->y;
        }

        int node_cost = 0;

        auto cci = moying_map_->corrected_cells(vehicle_);
        if(cci != nullptr){
            node_cost = (*cci)[mx][my].cost;
            //如果是等待,cell本身的cost不再叠加,只计算 pattern的cost
            if(local_path_plan_ && cur_node->x == next_node->x && cur_node->y == next_node->y && cur_node->th == next_node->th){
                node_cost = 0;
            }else{
                if(drive_on_side_){ //如果是靠边行驶,反转cell的cost,把靠墙的导航点cost设的最小
                    int free_node_cost = 5;
                    if(node_cost == 0)
                        node_cost = free_node_cost;
                    else{
                        node_cost = round(free_node_cost * 2 / node_cost) ;
                    }
                }
            }
        }

        node_cost += local_cost_map_->getCellCost(mx,my);
        next_node->g = cur_node->g + pattern.cost+node_cost;
        // next_node->g = cur_node->g + pattern.cost+node_cost;
        next_node->f = next_node->g + heuristic(next_node, goal_node_);
        next_node->sm_state = pattern.sm_state;
        next_node->parent = cur_node;

        return true;
    }

    bool TimeBasedAStar::closeToGoal(const NodePtr &curr, const NodePtr &goal)
    {
        return curr->x == goal->x && curr->y == goal->y && curr->th == goal->th;
    }

    void TimeBasedAStar::displayPath(const PlanningResult& result){
        // for(auto node : result.node_path){
        //     HIGHLIGHT("robot_id:"<<vehicle_->getID()<<" x:"<<node.x<<" y:"<<node.y<<" th:"<<(int)node.th<<" sm:"<<(int)node.sm_state<<" t:"<<node.t);
        // }
        
    }

    bool TimeBasedAStar::getMoveToNeighbourPath(const VehicleState& cur_state,
                    const VehicleState& next_state,const NodePtr start_node,const NodePtr next_node,std::vector<Node>& node_vec,bool start_point){

        if(start_node == nullptr || next_node == nullptr){
            WARNING("getMoveToNeighbourPath return false")
            return false;
        }

        //当前点和下一点是否是同一个cell
        bool is_same_cell = start_node->x == next_node->x && start_node->y == next_node->y;
        NodePtr last_node = start_node;    
        int last_t = 0;
        float x1 = cur_state.x;
        float y1 = cur_state.y;
        float x2 = next_state.x;
        float y2 = next_state.y;
        float x_delta = fabs(x1-x2);
        float y_delta = fabs(y1-y2);
        float dis = sqrt(x_delta*x_delta + y_delta*y_delta); 

        //计算当前点与第二个cell连线的朝向
        float rotate_theta = 0;
        if(dis <= SAME_POINT_COOR_DIS){
            rotate_theta = cur_state.th;
            printf("dis <= SAME_POINT_COOR_DIS  start_x:%d,start_y:%d,next_x:%d,next_y:%d\n",start_node->x,start_node->y,next_node->x,next_node->y);
        }else{
            float cos_value = (x2-x1)/dis;
            cos_value = cos_value < -1 ? -1 : cos_value;
            cos_value = cos_value > 1 ? 1 : cos_value;
            rotate_theta = acos(cos_value);
            if(y2 < y1)
                rotate_theta = -rotate_theta;
        }

        float theta_dis = fabs(cur_state.th - rotate_theta);
        float theta_dis2 = fabs(next_state.th - rotate_theta);
        if(theta_dis > M_PI){
            theta_dis = M_PI*2-theta_dis; 
        }
        if(theta_dis2 > M_PI){
            theta_dis2 = M_PI*2-theta_dis2; 
        }

        if( start_point && theta_dis2 > SAME_POINT_THETA_DIS && next_node->sm_state == MovePatternState::kMoveForwardState){
            // WARNING("can't get the move pattern state of next node");
            return false;
        }

        auto cfg_info = sch::ScheduleConfig::instance()->getConfigInfo();
        int search_theta_num = cfg_info.traffic_manager_cfg.search_theta_num;

        auto getMovePattern = [this,search_theta_num](float theta_dis)->MovePattern{

            float min_dis = INT_MAX;
            int min_index = 0;
            MovePattern mp(0,0,0,0,0,kStaticState);
            if(search_theta_num == 4){
                float thetas[] = {M_PI_2,M_PI};
                MovePatternType mpt[4] = {kRotateLeft90Pattern,kRotate180Pattern};
                for(int i=0;i<2;i++){
                    float dis = fabs(theta_dis - thetas[i]);
                    if(dis < min_dis){
                        min_dis = dis;
                        min_index = i;
                        mp = Vehicle::getMovePattern(mpt[i]);
                    }
                } 
                return mp;

            }else if(search_theta_num == 8){

                float thetas[] = {M_PI_4,M_PI_2,M_PI_4*3,M_PI};
                MovePatternType mpt[4] = {kRotateLeft45Pattern,kRotateLeft90Pattern,kRotateLeft135Pattern,kRotate180Pattern};
                for(int i=0;i<4;i++){
                    float dis = fabs(theta_dis - thetas[i]);
                    if(dis < min_dis){
                        min_dis = dis;
                        min_index = i;
                        mp = Vehicle::getMovePattern(mpt[i]);
                    }
                } 
                return mp;
            }else{
                return mp;
            }


        };

        //旋转朝向第二个cell
        if( (!is_same_cell && theta_dis > 0.2) || (is_same_cell && theta_dis > SAME_POINT_THETA_DIS)){ //夹角大于指定值才旋转 ,1度=0.017

            if(last_node->sm_state == kMoveForwardState){  //如果当前节点是在运动状态,下一节点不允许旋转
                return false;
            }

            MovePattern move_pattern = getMovePattern(theta_dis);
            Node node = *last_node;
            node.th         = vehicle_->getCloseTheta(rotate_theta);
            node.t          = last_node->t + move_pattern.t;
            node.sm_state   = MovePatternState::kStaticState;
            node.theta      = rotate_theta;
            if(!calculateNodeCost(last_node,&node,move_pattern)){
                return false;
            }

            if(dimension_.time > 0 && node.t >= dimension_.time){
                // WARNING("node's t is exceed time dimension");
                return false;
            }

            if(!isNodeCanPass(last_node,&node)){
                if(!local_path_plan_ && !start_point){
                    printf("rotate node can't pass lt.x:%d,lt.y:%d,lt.th:%d,nt.x:%d,nt.y:%d,nt.th:%d robot_id:%d\n",last_node->x,last_node->y,(int)last_node->th,
                        node.x,node.y,(int)node.th,vehicle_->getID());
                }
                return false;
            }

            // if(last_node->x == node.x && last_node->x == 8 && last_node->y == node.y && last_node->y == 10 && last_node->th == 2 && node.th == 6)
            //     printf("----- 8 10 2 6 can rotate\n");

            node_vec.emplace_back(node);
            last_node = &node_vec.back();

        }

        //往前直行到第二个cell
        if(dis>SAME_POINT_COOR_DIS){

            int t = 0;
            float velocity = vehicle_->getVelocity(kMoveForwardState);
            velocity = velocity == 0 ? 0.5 : velocity;
            float time = dis/velocity;  
            time += 0.5; //加速度
            t = round(time);
            
            bool second_rotate = theta_dis2 > SAME_POINT_THETA_DIS;
            MovePatternState move_state = second_rotate || next_node->sm_state == MovePatternState::kStaticState 
                                             ? MovePatternState::kStaticState : MovePatternState::kMoveForwardState;
            MovePattern move_pattern = move_state == MovePatternState::kStaticState
                            ?  Vehicle::getMovePattern(kReduceSpeedPattern) : Vehicle::getMovePattern(kAcceleratePattern);
                            // ?  Vehicle::getMovePattern(kMoveForwardStopPattern) : Vehicle::getMovePattern(kAcceleratePattern);

            Node node = *next_node;
            node.th         = vehicle_->getCloseTheta(rotate_theta);
            node.t          = last_node->t + move_pattern.t;
            node.sm_state   = move_state;
            node.theta      = rotate_theta;
            if(!calculateNodeCost(last_node,&node,move_pattern)){
                printf("forward node can't pass\n");
                return false;
            }
            if(dimension_.time > 0 && node.t >= dimension_.time){
                // WARNING("node's t is exceed time dimension");
                return false;
            }

            if(!isNodeCanPass(last_node,&node)){
                if(!local_path_plan_ && !start_point){
                    printf("forword node can't pass lt.x:%d,lt.y:%d,lt.th:%d,nt.x:%d,nt.y:%d,nt.th:%d robot_id:%d\n",last_node->x,last_node->y,(int)last_node->th,
                        node.x,node.y,(int)node.th,vehicle_->getID());
                }
                return false;
            }

            node_vec.emplace_back(node);
            last_node = &node_vec.back();

        }

        //旋转到第二个cell的朝向
        if( (start_point && !is_same_cell && theta_dis2 > 0.2) || ( (!start_point || is_same_cell) && theta_dis2 > SAME_POINT_THETA_DIS) ){

            if(last_node->sm_state == kMoveForwardState){  //如果当前节点是在运动状态,下一节点不允许旋转
                return false;
            }

            MovePattern move_pattern = getMovePattern(theta_dis2);
            Node node = *next_node;
            if(last_node == start_node){    //前面两个步骤没有执行,在上一个节点坐标上旋转
                node = *last_node;
            }
            node.th         = next_node->th;
            node.t          = last_node->t + move_pattern.t;
            node.sm_state   = MovePatternState::kStaticState;
            node.theta      = next_state.th;
            if(!calculateNodeCost(last_node,&node,move_pattern)){
                return false;
            }
            if(dimension_.time > 0 && node.t >= dimension_.time){
                // WARNING("node's t is exceed time dimension");
                return false;
            }
            
            if(!isNodeCanPass(last_node,&node)){
                if(!local_path_plan_ && !start_point){
                    printf("second rotate node can't pass lt.x:%d,lt.y:%d,lt.th:%d,nt.x:%d,nt.y:%d,nt.th:%d robot_id:%d\n",last_node->x,last_node->y,(int)last_node->th,
                        node.x,node.y,(int)node.th,vehicle_->getID());
                }
                return false;
            }

            node_vec.emplace_back(node);
            last_node = &node_vec.back();
        }

        if(node_vec.empty())
            return false;

        PathEndNodeInfo info;
        info.parent = start_node;
        info.nodes  = node_vec;
        if(start_point){
            path_start_nodes_.emplace(node_vec.back().f,info);
        }else{
            path_end_nodes_.emplace(node_vec.back().t,info);
        }
        return true;

    }

}
}